from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription
from launch.substitutions import PathJoinSubstitution
from launch_ros.substitutions import FindPackageShare
from launch.actions import DeclareLaunchArgument
from launch.substitutions import LaunchConfiguration
from launch.actions import TimerAction
import os
import yaml

if os.path.exists('config.yaml'):
    config_file = 'config.yaml'
    with open(config_file, 'r', encoding='utf-8') as file:
        config = yaml.safe_load(file)
    
    # 如果 config.yaml 中没有 px4_paths，则读取 config_template.yaml
    if "px4_paths" not in config or not config["px4_paths"]:
        if os.path.exists('config_template.yaml'):
            with open('config_template.yaml', 'r', encoding='utf-8') as file:
                config = yaml.safe_load(file)
        else:
            raise KeyError("config.yaml 中没有 px4_paths 且找不到 config_template.yaml")

# 如果 config.yaml 不存在，则读取 config_template.yaml
elif os.path.exists('config_template.yaml'):
    config_file = 'config_template.yaml'
    with open(config_file, 'r', encoding='utf-8') as file:
        config = yaml.safe_load(file)
else:
    raise FileNotFoundError("找不到配置文件：config.yaml 或 config_template.yaml")

config["px4_paths"]

def generate_launch_description():
    px4_path = LaunchConfiguration('px4_path')
    ugv_ids = ['ugv1']
    px4_path_launch_arg = DeclareLaunchArgument(
        'px4_path',
        default_value= config["px4_paths"],
        description='PX4 Firmware Path'
    )
    
    launch_dir = PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch'])
    return LaunchDescription([
        px4_path_launch_arg,

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'px4uav.launch.py']),
            launch_arguments={'uav_id': 'uav1', 'uav_pose': '0,0,0.5', 'uav_num': '0', 'px4_path': px4_path}.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'gzpx.launch.py']),
            launch_arguments={'world_sdf': 'default.sdf','bridge_config':'one_ugv_bridge.yaml'}.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([FindPackageShare('sls_launch_sim'), 'launch', 'gzugv.launch.py']),
            launch_arguments={
                'ugv_id': 'ugv1',
                'ugv_pose': '2.0,2.0,1.5,0,0,0'
            }.items()
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'microagent.launch.py'])
        ),
        

        # TimerAction(
        #     period=3.0,  # 延迟时间，单位为秒
        #     actions=[
        #         IncludeLaunchDescription(
        #             PathJoinSubstitution([launch_dir, 'uav.launch.py']),
        #             launch_arguments={
        #                 'vehicleID': 'uav1',
        #                 'local_host_mode': 'True',
        #                 'multi_simulation_mode': 'True'
        #             }.items()
        #         ),
        #     ]
        # ),

        TimerAction(
            period=3.0,  # 延迟时间，单位为秒
            actions=[
                IncludeLaunchDescription(
                    PathJoinSubstitution([launch_dir, 'ugv.launch.py']),
                    launch_arguments={
                        'vehicleID': 'ugv1',
                        'local_host_mode': 'True',
                        'multi_simulation_mode': 'True',
                        'ugv_simulation_mode':'True'
                    }.items()
                )
            ]
        ),

        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'ugv_allstart.launch.py']),
            launch_arguments={
                'ugv_ids': str(ugv_ids)  
            }.items()
        ),
        
        IncludeLaunchDescription(
            PathJoinSubstitution([launch_dir, 'addi_ugv1_bridge.launch.py'])
        )

    ])